#include "pathFinding.h"
#include "pathNode.h"
#include <list>
#include <iostream>
#include <queue>
#include <vector>

using std::list;
using std::cout;
using std::endl;
using std::priority_queue;
using std::vector;





//CONSTRUCTORS
Path::Path(int w, int h)
{
    width = w;
    height = h;
    data = new Node*[h];
    for (int x = 0; x < h; x++)
    {
        data[x] = new Node[w];
        for (int y = 0; y < w; y++)
        {
            data[x][y] = Node(y,x);
        }
    }
}

Path::~Path()
{
    for (int x = 0; x < height; x++)
    {
        delete [] data[x];
    }
    delete [] data;
}

//ACCESSORS
int Path::getw()
{
    return width;
}

int Path::geth()
{
    return height;
}

bool Path::getPath(int startx, int starty, int endx, int endy, list<int> & ret)
{
    while (!open.empty())
        open.pop();
    while (!closed.empty())
        closed.pop();
    
    data[starty][startx].setg(0);
    data[starty][startx].calch(endx,endy);
    data[starty][startx].setp(NULL);
    open.push(&data[starty][startx]);
    Node* selected = &(data[starty][startx]);
    while (true)
    {
        //cout << "===STARTING PASS===" << endl;
        selected = open.top();
        //cout << "Top of open list: " << endl;
        //selected->print();
        closed.push(selected);
        open.pop();

        list<Node*> adj =  getAdj(selected->getx(), selected->gety(), closed);
        //cout << "Num of Adj: " << adj.size() << endl;
        for (list<Node*>::iterator it = adj.begin(); it != adj.end(); it++)
        {
            if (!contained(**it, open))
            {
                (*it)->setg(selected->getg() + getCost(selected, (*it)));
                (*it)->calch(endx, endy);
                (*it)->setp(selected);
                open.push(*it);
		
            } else {
                int newg = selected->getg() + getCost(selected, (*it));
                if (newg < (*it)->getg())
                {
                    (*it)->setg(newg);
                    (*it)->setp(selected);
                }
            }
        }


        //test if destination is found
        if (contained(data[endy][endx], closed))
        {
            Node* curnode = &(data[endy][endx]);
            while (curnode->getp() != NULL)
            {
                ret.push_front(getDir(curnode, curnode->getp()));
                curnode = curnode->getp();
            }
            //TODO:: POPULATE RET
            return true;
        }
        //Failed to find path
        if (open.size() == 0)
        {
            return false;
        }
    }
}

list<Node*> Path::getAdj(int x, int y, priority_queue<Node*, vector<Node*>, cmpPQ> & closed)
{
    //cout << "getting adj for (" << x << "," << y << ")...";
    list<Node*> ret;
    for (int a = x-1; a <= x+1; a++)
    {
        for (int b = y-1; b <= y+1; b++)
        {
            //cout << "Testing Adj (" << a << "," << b << ")...";
            if ((a >= 0 && a < width) && (b >= 0 && b < height) && (!(a == x && b == y)))
            {
                if (data[b][a].getw() && iscanDiag(x,y,a,b))
                {
                    if (!contained(data[b][a], closed))
                    {
                        //cout << "(" << a << "," << b << ")";
                        ret.push_back(&data[b][a]);
                    } else {
                        //cout << "Already in closed" << endl;
                    }
                } else {
                    //cout << "Fail Walkable" << endl;
                }
            } else {
                //cout << "Fail Map Bounds" << endl;
            }
        }
    }
    //cout << endl;
    return ret;
}

bool Path::iscanDiag(int x, int y, int a, int b)
{
    int test = 0;
    if (x != a)
        test += 10;
    if (y != b)
        test += 10;
    if (test != 20) // NOT Diagonal
        return true;
    bool ret = true;
    if (!data[y][a].getw())
        ret = false;
    if (!data[b][x].getw())
        ret = false;
    return ret;
}

bool Path::contained(Node & n, priority_queue<Node*, vector<Node*>, cmpPQ> l)
{
    while (l.size() > 0)
    {
        if (*(l.top()) == n)
            return true;
        if (*(l.top()) > n)
            return false;
        l.pop();
    }
    return false;
}

Node* Path::getLowest(list<Node*> & l)
{
    list<Node*>::iterator ret = l.begin();
    for (list<Node*>::iterator low = l.begin(); low != l.end(); low++)
    {
        if ((**low) < (**ret))
            ret = low;
    }
    return (*ret);
}

int Path::getCost(Node* parent, Node* child)
{
    int ret = 0;
    if (parent->getx() != child->getx())
        ret += 10;
    if (parent->gety() != child->gety())
        ret += 10;
    if (ret == 20) // Diagonal
        ret = 14;
    return ret;
}

int Path::getDir(Node* parent, Node* child)
{
    //cout << "getting dir (" << parent->getx() << "," << parent->gety() << ") to (" << child->getx() << "," << child->gety() << ")...";
    //Y
    int ret = 0;
    if (parent->gety() > child->gety())
        ret = 2;
    if (parent->gety() == child->gety())
        ret = 5;
    if (parent->gety() < child->gety())
        ret = 8;
    //X
    if (parent->getx() < child->getx())
        ret--;
    if (parent->getx() > child->getx())
        ret++;
    //cout << ret << endl;
    return ret;
}

//MODIFIERS
//OPERATORS

//OTHER
void Path::printPath(list<int> & retpath, int startx, int starty, int endx, int endy)
{
    char** outp = new char*[height];
    for (int y = 0; y < height; y++)
    {
        outp[y] = new char[width];
        for (int x = 0; x < width; x++)
        {
            if (data[y][x].getw())
                outp[y][x] = '0';
            else
                outp[y][x] = '|';
        }
    }
    while (!open.empty())
    {
        Node* curOpenNode = open.top();
        outp[curOpenNode->gety()][curOpenNode->getx()] = 'x';
        open.pop();
    }
    while (!closed.empty())
    {
        Node* curClosedNode = closed.top();
        outp[curClosedNode->gety()][curClosedNode->getx()] = 'y';
        closed.pop();
    }
    outp[starty][startx] = '*';
    outp[endy][endx] = '*';
    int curx = startx;
    int cury = starty;
    for (list<int>::iterator it = retpath.begin(); it != retpath.end(); it++)
    {
        //cout << *it;
        switch (*it)
        {
          case 1:
            cury++;
            curx--;
            break;
          case 2:
            cury++;
            break;
          case 3:
            cury++;
            curx++;
            break;
          case 4:
            curx--;
            break;
          case 6:
            curx++;
            break;
          case 7:
            curx--;
            cury--;
            break;
          case 8:
            cury--;
            break;
          case 9:
            cury--;
            curx++;
            break;
        }
        outp[cury][curx] = '*';
    }
    //cout << endl;



    //PRINT
    for (int y = 0; y < height; y++)
    {
        for (int x = 0; x < width; x++)
        { 
            cout << outp[y][x];
        }
        cout << endl;
    }
}
